/**
  ******************************************************************************
  * @file    ctrl.c
  * @author  Tmax Sco
  * @version V1.0.0
  * @date    2017-2-22
  * @brief   
  ******************************************************************************
  */
/* Includes -------------------------------------------------------------------*/
#include "ctrl.h"
#include "math.h"
#include "rm_motor.h"
#include "gpio.h"
#include "usart.h"
#include "comm.h"
#include "timer.h"
#include "toolBoxScope.h"
#include "string.h"
/* Private  typedef -----------------------------------------------------------*/
/* Private  define ------------------------------------------------------------*/
/* Private  macro -------------------------------------------------------------*/
/* Private  variables ---------------------------------------------------------*/
DriverType Driver[8] = {0};
/* Extern   variables ---------------------------------------------------------*/

/* Extern   function prototypes -----------------------------------------------*/
/* Private  function prototypes -----------------------------------------------*/
/* Private  functions ---------------------------------------------------------*/
#define BOARD AUTO_3508
int waveNum = 0; /* 0 is off */
float STVEL = 200; /* 射击速度 */
int32_t maxShootVel = 0; /* 设计最大速度 */

/* TODO 安全模式开发(1s未收到主控信息断电) */
int safe_mode_flag = 0;

/* FIXME Roboncup2020使用的两块驱动板定义 */

#ifdef UP
int motorType[8] = {
	0,
	0,
	0,
	0,
	GM_6020,
	0,
	0,
	0};
#else
int motorType[8] = {
	RM_3508,
	RM_3508,
	RM_3508,
	RM_3508,
	0,
	0,
	0,
	0};
#endif

/**
 * @brief 3508初始化
 * 
 * @param driver 
 * @param mode 模式选择
 */
void RM_3508_init(DriverType *driver, uint8_t mode)
{
	memset(&(driver->posCtrl), 0, sizeof(PosCtrlType));
	memset(&(driver->curCtrl), 0, sizeof(CurCtrlType));
	memset(&(driver->velCtrl), 0, sizeof(VelCtrlType));

	driver->unitMode = mode;

	driver->velCtrl.kp = VEL_KP_3508;
	driver->velCtrl.ki = VEL_KI_3508;
	driver->velCtrl.maxOutput = CURRENT_MAX_3508;
	driver->velCtrl.desiredVel[MAX_V] = VEL_MAX_3508;
	driver->posCtrl.kd = POS_KD_3508;
	driver->posCtrl.kp = POS_KP_3508;
	driver->curCtrl.maxCur = CURRENT_MAX_3508;
	driver->homingMode.current =1.0f;

	driver->velCtrl.acc = 1000.0f;
	driver->velCtrl.dec = 1000.0f;
	driver->velCtrl.desiredVel[CMD] = 0.0f;
	driver->posCtrl.acc = driver->velCtrl.dec;
	driver->posCtrl.posVel = 50.0f;
	// driver->posCtrl.posVel = 1300.0f;
	driver->posCtrl.actualPos = 0.0f;
	driver->curCtrl.desireCur = 0.0f;
	driver->homingMode.vel = -100.0f;
	driver->encoder.period = 8192;
}

void M_2006_init(DriverType *driver, uint8_t mode)
{
	memset(&(driver->posCtrl), 0, sizeof(PosCtrlType));
	memset(&(driver->curCtrl), 0, sizeof(CurCtrlType));
	memset(&(driver->velCtrl), 0, sizeof(VelCtrlType));
	driver->unitMode = mode;

	driver->velCtrl.kp = VEL_KP_2006;
	driver->velCtrl.ki = VEL_KI_2006;
	driver->velCtrl.maxOutput = CURRENT_MAX_2006;
	driver->velCtrl.desiredVel[MAX_V] = VEL_MAX_2006;
	driver->posCtrl.kd = POS_KD_2006;
	driver->posCtrl.kp = POS_KP_2006;
	driver->curCtrl.maxCur = CURRENT_MAX_2006;
	driver->homingMode.current = 0.8f;

	driver->velCtrl.acc = 1000.0f;
	driver->velCtrl.dec = 1000.0f;
	driver->velCtrl.desiredVel[CMD] = 0.0f;
	driver->posCtrl.acc = driver->velCtrl.dec;
	driver->posCtrl.posVel = 500.0f;
	driver->posCtrl.actualPos = 0.0f;
	driver->curCtrl.desireCur = 0.0f;
	driver->homingMode.vel = -160.0f;
	driver->encoder.period = 8192;
}

void GM_6020_init(DriverType *driver, uint8_t mode)
{
	memset(&(driver->posCtrl), 0, sizeof(PosCtrlType));
	memset(&(driver->curCtrl), 0, sizeof(CurCtrlType));
	memset(&(driver->velCtrl), 0, sizeof(VelCtrlType));
	driver->unitMode = mode;

	driver->velCtrl.kp = VEL_KP_6020;
	driver->velCtrl.ki = VEL_KI_6020;
	driver->velCtrl.maxOutput = VOLTAGE_MAX_6020;
	driver->velCtrl.desiredVel[MAX_V] = VEL_MAX_6020;
	driver->posCtrl.kd = POS_KP_6020;
	driver->posCtrl.kp = POS_KD_6020;
	driver->curCtrl.maxCur = VOLTAGE_MAX_6020;
	driver->homingMode.current = 2.0f;

	driver->velCtrl.acc = 1000.0f;
	driver->velCtrl.dec = 1000.0f;
	driver->velCtrl.desiredVel[CMD] = 0.0f;
	driver->posCtrl.acc = driver->velCtrl.dec;
	driver->posCtrl.posVel = 30.0f;
	driver->posCtrl.actualPos = 0.0f;
	driver->curCtrl.desireCur = 0.0f;
	driver->homingMode.vel = -160.0f;
	driver->encoder.period = 8192;
}

/**
 * @brief 初始化ShootMode
 * 
 */
void ShootMode_init(ShootModeType *shootmode, float vel, float rotatePos)
{
	memset(shootmode, 0, sizeof(ShootModeType));
	shootmode->rotatePos = rotatePos;
	shootmode->vel = vel;
	shootmode->current = CURRENT_MAX_3508;
	maxShootVel = 0;
}

/**
  * @brief  初始化驱动器
  * @param  None
  * @retval 
  */
void DriverInit(void)
{
	for (int i = 0; i < 8; i++)
	{
		Driver[i].command.canId = i + 1;
		Motor[i].type = motorType[i];
		if (Motor[i].type == RM_3508)
		{
			RM_3508_init(&Driver[i], SPEED_CONTROL_MODE);
			MotorOn(i);
		}
		else if (Motor[i].type == M_2006)
		{
			M_2006_init(&Driver[i], POSITION_CONTROL_MODE);
			MotorOn(i);
		}
		else if (Motor[i].type == GM_6020)
		{
			GM_6020_init(&Driver[i], POSITION_CONTROL_MODE);
			MotorOn(i);
		}
		else
		{
			MotorOff(i);
		}
	}
}

/**
  * @brief  MotorCtrl
	* @param  None
	* @retval None
  */
float PerCur[8] = {0.0f};
float wave[4];
void MotorCtrl(void)
{
	for (int i = 0; i < 8; i++)
	{
		if (Motor[i].type == NONE)
			continue;

		CalculSpeed_Pos(&Driver[i], &Motor[i]);

		if (Driver[i].status != ENABLE)
		{
			Driver[i].output = 0.0f;
			continue;
		}

		switch (Driver[i].unitMode)
		{
		case POSITION_CONTROL_MODE:
			PosCtrl(&Driver[i].posCtrl);
			Driver[i].velCtrl.desiredVel[CMD] = Driver[i].posCtrl.output;
			VelSlope(&Driver[i].velCtrl);
			Driver[i].output = VelPidCtrl(&Driver[i].velCtrl);
			break;
		case SPEED_CONTROL_MODE:
			//			Driver[i].output = VelCtrl(VelSlope(Driver[i].velCtrl.desiredVel[CMD]));
			VelSlope(&Driver[i].velCtrl);
			Driver[i].output = VelPidCtrl(&Driver[i].velCtrl);
			break;
		case CUR_CONTROL_MODE:
			Driver[i].output = Driver[i].curCtrl.desireCur;
			break;
		case HOMING_MODE:
			HomingMode(&Driver[i]);
			Driver[i].output = Driver[i].homingMode.output;
			break;
		case SHOOT_MODE:
			ShootMode(&Driver[i]);
			Driver[i].output = Driver[i].shootMode.output;
		default:
			break;
		}
	}

	for (int i = 0; i < 8; i++)
	{
		if (Motor[i].type == RM_3508)
			PerCur[i] = Driver[i].output * 16384.0f / 20.0f;
		else if (Motor[i].type == M_2006)
			PerCur[i] = Driver[i].output * 10000.0f / 10.0f; //M2006
		else if (Motor[i].type == GM_6020)
			PerCur[i] = Driver[i].output * 30000.0f / 30.f; //������ʵ�ǵ�ѹ�����Ǻ͵��������߼���һ����
		else
			PerCur[i] = 0.0f;
	}
	static int putcnt = 0;
	SetCur(PerCur);

	if (waveNum && putcnt % 20 == 0) //?????
	{

		wave[0] = Driver[waveNum - 1].velCtrl.speed;
		wave[1] = Driver[waveNum - 1].posCtrl.actualPos;
		wave[2] = Motor[waveNum - 1].vel;

		wave[3] = PerCur[waveNum - 1];

		toolBox_scope(wave, 3);
	}
	putcnt++;
}
/**
  * @brief  ???��??????
  * @param  None
  * @retval ??????????
  */
float VelSlope(VelCtrlType *velPid)
{
	/*************?????????��??**************/
	if (velPid->desiredVel[SOFT] < (velPid->desiredVel[CMD] - velPid->acc))
	{
		velPid->desiredVel[SOFT] += velPid->acc;
	}
	else if (velPid->desiredVel[SOFT] > (velPid->desiredVel[CMD] + velPid->dec))
	{
		velPid->desiredVel[SOFT] -= velPid->dec;
	}
	else
	{
		velPid->desiredVel[SOFT] = velPid->desiredVel[CMD];
	}
	velPid->desiredVel[SOFT] = velPid->desiredVel[CMD]; //auther: LJunius
	return velPid->desiredVel[SOFT];
}

/**
  * @brief  ??????
  * @param  None
  * @retval ???PID?????
  */
float VelPidCtrl(VelCtrlType *velPid)
{
	/*****************????PID*****************/
	velPid->velErr = velPid->desiredVel[SOFT] - velPid->speed;
	//???????
	velPid->iOut += velPid->ki * velPid->velErr;
	//???????
	velPid->iOut = MaxMinLimit(velPid->iOut, velPid->maxOutput);
	//???????
	velPid->output = velPid->kp * velPid->velErr + velPid->iOut;
	//??????
	velPid->output = MaxMinLimit(velPid->output, velPid->maxOutput);

	return velPid->output;
}

/**
  * @brief  ??????????
  * @param  val???????
  * @retval ????
  */
float OutPutLim(float value)
{
	float outputMax, outputMin, outputBasic;
	/********************??????????��???****************************/
	outputBasic = Driver[0].velCtrl.speed * EMF_CONSTANT; //?????�T??
	outputMax = outputBasic + VOL_AMP;					  //???????
	outputMin = outputBasic - VOL_AMP;					  //????????????????????
	if (outputMax < VOL_AMP)
		outputMax = VOL_AMP; //
	if (outputMin > -VOL_AMP)
		outputMin = -VOL_AMP;

	if (value < outputMin)
		value = outputMin; //
	if (value > outputMax)
		value = outputMax;

	if (value > VOL_MAX)
		value = VOL_MAX;
	if (value < -VOL_MAX)
		value = -VOL_MAX;

	//	CurrentOutput = (value - (float)velpms*0.04315f)*25.0f;
	if (value < 0)
		value -= VOL_BLIND_AREA; //???????????0.3043f Vq??????tim4
	else
		value += VOL_BLIND_AREA;

	return value;
}

/**
  * @brief  ��?????(??��???????)
  * @param  None
  * @retval ��???PID???????
  */
float PosCtrl(PosCtrlType *posPid)
{
	float posPidOut = 0.0f;
	float desiredVel = 0.0f, signVel = 1.0f;

	/******************************????��??????**************************************/
	posPid->posErr = posPid->desiredPos - posPid->actualPos;
	posPidOut = posPid->posErr * posPid->kp + posPid->kd * (posPid->posErr - posPid->posErrLast);
	posPid->posErrLast = posPid->posErr;

	if (posPid->posErr < 0.0f)
		signVel = -1.0f;

	//????0.7????????????????????��??????��??????
	desiredVel = signVel * __sqrtf(2.0f * 0.7f * posPid->acc * signVel * posPid->posErr);

	if (fabsf(desiredVel) < fabsf(posPidOut))
		posPidOut = desiredVel;
	//???????��??????
	//	if(fabsf(posPid->posErr) <= 200.0f)		posPidOut = 0.0f;

	posPid->output = MaxMinLimit(posPidOut, posPid->posVel);

	return posPid->output;
}
/**
  * @brief  Homing mode
  * @param  None
  * @retval ??????
  */

void HomingMode(DriverType *driver)
{

	// static int testValue = 0;
	
	
	float output;

	driver->velCtrl.desiredVel[SOFT] = driver->homingMode.vel;
	output = VelPidCtrl(&driver->velCtrl);
	driver->homingMode.output = MaxMinLimit(output, driver->homingMode.current); //????home????????

	if (fabsf(driver->velCtrl.speed) <= 2)
	{ //2
		driver->homingMode.cnt++;
	}
	else
	{
		driver->homingMode.cnt = 0;
	}

	if (driver->homingMode.cnt >= 500)
	{ //500ms
		USART_OUT(USART3, "HW END=%d\r\n",(int)(driver->posCtrl.actualPos));
		memset(&(driver->homingMode),0,sizeof(HomingModeType));
		driver->posCtrl.actualPos = 0.0f; //
		driver->posCtrl.desiredPos = 0.0f;
		driver->velCtrl.desiredVel[CMD] = 0.0f;
		driver->velCtrl.desiredVel[SOFT] = 0.0f;
		driver->velCtrl.output = 0.0f;
		driver->output = 0.0f;
		driver->velCtrl.iOut = 0.0f;
		driver->unitMode = POSITION_CONTROL_MODE;
		
	}
}

void ShootMode(DriverType *driver)
{
	
	if (fabsf(driver->posCtrl.actualPos) > driver->shootMode.rotatePos)
	{
		driver->velCtrl.desiredVel[SOFT] = 0;
		driver->shootMode.output = VelPidCtrl(&driver->velCtrl);
	}
	else
	{
		float output;
		driver->velCtrl.desiredVel[SOFT] = driver->shootMode.vel;
		output = VelPidCtrl(&driver->velCtrl);
		driver->shootMode.output = MaxMinLimit(output, driver->shootMode.current);
		// driver->shootMode.output = MaxMinLimit(CURRENT_MAX_3508, CURRENT_MAX_3508);
	}

	if (fabsf(driver->velCtrl.speed) <= 2)
	{ //2
		driver->shootMode.cnt++;
	}
	else
	{
		driver->shootMode.cnt = 0;
	}

	if (driver->shootMode.cnt >= 500)
	{//500ms
		maxShootVel = (float)(maxShootVel) * 0.0526315f;
		USART_OUT(USART3, "ST END=%d\r\n",(int)(driver->posCtrl.actualPos));
		USART_OUT(USART3, "maxVel=%d\r\n",(int32_t)(maxShootVel));

		RM_3508_init(driver,HOMING_MODE);
	}
}

/**
  * @brief  ??????????
  * @param  None
  * @retval ��?????????
  */
float GetPosPidOut(void)
{
	return Driver[0].posCtrl.output;
}

/**
  * @brief  Calculate Speed
  * @param  None
  * @retval Subtraction number between every two times.
**/
float CalculSpeed_Pos(DriverType *driver, MotorType *motor)
{
	int deltaPos = 0;
	deltaPos = (motor->pos - motor->posLast);
	motor->posLast = motor->pos;
	if (deltaPos > (driver->encoder.period / 2))
		deltaPos -= driver->encoder.period;
	if (deltaPos < -(driver->encoder.period / 2))
		deltaPos += driver->encoder.period;

	driver->posCtrl.actualPos += deltaPos;

	//????/ms
	driver->velCtrl.speed = (float)(motor->vel) * 0.1365333f; //1/60*8192/1000=0.136533
	//rpm?????
	// driver->velCtrl.speed = motor->vel * 0.0526315f;

	return driver->velCtrl.speed;
}

/**
  * @brief  Get Speed
  * @param  None
  * @retval Speed
**/
float GetSpeed(void)
{
	return Driver[0].velCtrl.speed;
}

/**
  * @brief  ??????????
  * @param  None
  * @retval ??????
  */
float GetVelPidOut(void)
{
	return Driver[0].velCtrl.output;
}

/**
  * @brief  max min limit
	* @param  inDat:
	* @retval outDat
  */
float MaxMinLimit(float val, float limit)
{
	if (val > limit)
		val = limit;
	if (val < -limit)
		val = -limit;

	return val;
}

/**
  * @brief  ??????
  * @param  n:??????  (0-7)
	* @retval None
  */
void MotorOn(int n)
{
	if (Driver[n].unitMode == POSITION_CONTROL_MODE)
		Driver[n].posCtrl.desiredPos = Driver[n].posCtrl.actualPos;

	if (Driver[n].unitMode == SPEED_CONTROL_MODE)
		Driver[n].velCtrl.desiredVel[CMD] = 0.0f;

	Driver[n].velCtrl.iOut = 0.0f;

	Driver[n].status = ENABLE;

	uint16_t pin = 1 << (n % 4);
	if ((motorType[0] != 0 && n <= 3) || (motorType[0] == 0 && n >= 4))
	{ /* 0~3??4~8???????????��LJunius */
		GPIO_WriteBit(GPIOA, pin, Bit_RESET);
	}
}

/**
  * @brief  ??????
  * @param  n:??????  (0-7)
	* @retval None
  */
void MotorOff(int n)
{
	Driver[n].status = DISABLE;
	uint16_t pin = 1 << (n % 4);
	int tmp = motorType[0] + motorType[1] + motorType[2] + motorType[3];
	if ((tmp != 0 && n <= 3) || (tmp == 0 && n >= 4))
	{ /* 0~3??4~8???????????��LJunius */
		GPIO_WriteBit(GPIOA, pin, Bit_SET);
	}
}

/**
  * @brief  ????????
	* @param  vel????????????��
	* @param  tim??????��????
	* @retval None
  */
void VelCtrlTest(float vel, int tim)
{
	Driver[0].velCtrl.desiredVel[CMD] = vel;
	TIM_Delayms(TIM3, tim);
	Driver[0].velCtrl.desiredVel[CMD] = -vel;
	TIM_Delayms(TIM3, tim);
}

/************************ (C) COPYRIGHT 2019 ACTION ********************/
